{
  "cells": [
    {
      "execution_count": null,
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      },
      "source": [
        "%matplotlib inline"
      ]
    },
    {
      "source": [
        "\n********************************************************************************\n3D Attitude Estimation - Benchmark\n********************************************************************************\nGoals of this script:\n\n* implement two different UKFs on the 3D attitude estimation example.\n\n* design the Extended Kalman Filter (EKF).\n\n* compare the different algorithms with Monte-Carlo simulations.\n\n*We assume the reader is already familiar with the considered problem described\nin the related example.*\n\nFor the given problem, two different UKFs emerge, defined respectively as:\n\n1- The state is embedded in $SO(3)$ with left multiplication, i.e.\n\n* the retraction $\\varphi(.,.)$ is the $SO(3)$ exponential where\n  uncertainty is multiplied on the left by the state.\n\n* the inverse retraction $\\varphi^{-1}(.,.)$ is the $SO(3)$\n  logarithm.\n\n2- The state is embedded in $SO(3)$ with right multiplication, i.e.\n\n* the retraction $\\varphi(.,.)$ is the $SO(3)$ exponential where\n  uncertainty is multiplied on the right by the state.\n\n* the inverse retraction $\\varphi^{-1}(.,.)$ is the $SO(3)$\n  logarithm.\n\nWe tests the different algorithms with the same noise parameter setting and on\nsimulation with moderate initial heading error.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "source": [
        "Import\n==============================================================================\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "execution_count": null,
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      },
      "source": [
        "from scipy.linalg import block_diag\nfrom ukfm import SO3, UKF, EKF\nfrom ukfm import ATTITUDE as MODEL\nimport ukfm\nimport numpy as np\nimport matplotlib\nukfm.set_matplotlib_config()"
      ]
    },
    {
      "source": [
        "Simulation Setting\n==============================================================================\nWe compare the filters on a large number of Monte-Carlo runs.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "execution_count": null,
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      },
      "source": [
        "# Monte-Carlo runs\nN_mc = 100"
      ]
    },
    {
      "source": [
        "This script uses the :meth:`~ukfm.ATTITUDE` model. The initial values of the\nheading error has 10\u00b0 standard deviation.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "execution_count": null,
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      },
      "source": [
        "# sequence time (s)\nT = 100\n# IMU frequency (Hz)\nimu_freq = 100\n# IMU noise standard deviation (noise is isotropic)\nimu_std = np.array([5/180*np.pi,  # gyro (rad/s)\n                    0.4,          # accelerometer (m/s**2)\n                    0.3])         # magnetometer\n# create the model\nmodel = MODEL(T, imu_freq)\n# propagation noise covariance matrix\nQ = imu_std[0]**2*np.eye(3)\n# measurement noise covariance matrix\nR = block_diag(imu_std[1]**2*np.eye(3), imu_std[2]**2*np.eye(3))\n# initial uncertainty matrix\nP0 = (10/180*np.pi)**2*np.eye(3)  # The state is perfectly initialized\n# sigma point parameters\nalpha = np.array([1e-3, 1e-3, 1e-3])"
      ]
    },
    {
      "source": [
        "Filter Design\n==============================================================================\nAdditionally to the UKFs, we compare them to an EKF. The EKF has the same\nuncertainty representation as the UKF with right uncertainty representation.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "source": [
        "We set variables for recording metrics before launching Monte-Carlo\nsimulations.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "execution_count": null,
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      },
      "source": [
        "left_ukf_err = np.zeros((N_mc, model.N, 3))\nright_ukf_err = np.zeros_like(left_ukf_err)\nekf_err = np.zeros_like(left_ukf_err)\n\nleft_ukf_nees = np.zeros((N_mc, model.N))\nright_ukf_nees = np.zeros_like(left_ukf_nees)\nekf_nees = np.zeros_like(left_ukf_nees)"
      ]
    },
    {
      "source": [
        "Monte-Carlo Runs\n==============================================================================\nWe run the Monte-Carlo through a for loop.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "execution_count": null,
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      },
      "source": [
        "for n_mc in range(N_mc):\n    print(\"Monte-Carlo iteration(s): \" + str(n_mc+1) + \"/\" + str(N_mc))\n    # simulate true states and noisy inputs\n    states, omegas = model.simu_f(imu_std)\n    # simulate accelerometer and magnetometer measurements\n    ys = model.simu_h(states, imu_std)\n    # initial state with error\n    state0 = model.STATE(Rot=states[0].Rot.dot(\n        SO3.exp(10/180*np.pi*np.random.randn(3))))\n    # covariance need to be \"turned\"\n    left_ukf_P = state0.Rot.dot(P0).dot(state0.Rot.T)\n    right_ukf_P = P0\n    ekf_P = P0\n\n    # variables for recording estimates of the Monte-Carlo run\n    left_ukf_states = [state0]\n    right_ukf_states = [state0]\n    ekf_states = [state0]\n\n    left_ukf_Ps = np.zeros((model.N, 3, 3))\n    right_ukf_Ps = np.zeros_like(left_ukf_Ps)\n    ekf_Ps = np.zeros_like(left_ukf_Ps)\n\n    left_ukf_Ps[0] = left_ukf_P\n    right_ukf_Ps[0] = right_ukf_P\n    ekf_Ps[0] = ekf_P\n\n    left_ukf = UKF(state0=states[0], P0=P0, f=model.f, h=model.h, Q=Q, R=R,\n                   phi=model.phi,\n                   phi_inv=model.phi_inv,\n                   alpha=alpha)\n    right_ukf = UKF(state0=states[0], P0=P0, f=model.f, h=model.h, Q=Q, R=R,\n                    phi=model.right_phi,\n                    phi_inv=model.right_phi_inv,\n                    alpha=alpha)\n    ekf = EKF(model=model, state0=states[0], P0=P0, Q=Q, R=R,\n              FG_ana=model.ekf_FG_ana,\n              H_ana=model.ekf_H_ana,\n              phi=model.right_phi)\n    # filtering loop\n    for n in range(1, model.N):\n        # propagation\n        left_ukf.propagation(omegas[n-1], model.dt)\n        right_ukf.propagation(omegas[n-1], model.dt)\n        ekf.propagation(omegas[n-1], model.dt)\n        # update\n        left_ukf.update(ys[n])\n        right_ukf.update(ys[n])\n        ekf.update(ys[n])\n        # save estimates\n        left_ukf_states.append(left_ukf.state)\n        right_ukf_states.append(right_ukf.state)\n        ekf_states.append(ekf.state)\n        left_ukf_Ps[n] = left_ukf.P\n        right_ukf_Ps[n] = right_ukf.P\n        ekf_Ps[n] = ekf.P\n    # \u00a0get state\n    Rots, _ = model.get_states(states, model.N)\n    left_ukf_Rots, _ = model.get_states(left_ukf_states, model.N)\n    right_ukf_Rots, _ = model.get_states(right_ukf_states, model.N)\n    ekf_Rots, _ = model.get_states(ekf_states, model.N)\n    # record errors\n    left_ukf_err[n_mc] = model.errors(Rots, left_ukf_Rots)\n    right_ukf_err[n_mc] = model.errors(Rots, right_ukf_Rots)\n    ekf_err[n_mc] = model.errors(Rots, ekf_Rots)\n    # record NEES\n    left_ukf_nees[n_mc] = model.nees(left_ukf_err[n_mc], left_ukf_Ps,\n                                     left_ukf_Rots, 'LEFT')\n    right_ukf_nees[n_mc] = model.nees(right_ukf_err[n_mc], right_ukf_Ps,\n                                      right_ukf_Rots, 'RIGHT')\n    ekf_nees[n_mc] = model.nees(ekf_err[n_mc], ekf_Ps, ekf_Rots, 'RIGHT')"
      ]
    },
    {
      "source": [
        "Results\n==============================================================================\nWe visualize the results averaged over Monte-Carlo sequences, and compute the\nRoot Mean Squared Error (RMSE) averaged over all Monte-Carlo.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "execution_count": null,
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      },
      "source": [
        "model.benchmark_print(left_ukf_err, right_ukf_err, ekf_err)"
      ]
    },
    {
      "source": [
        "All the curves have the same shape. Filters obtain the same performances.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "source": [
        "We finally compare the filters in term of consistency (Normalized Estimation\nError Squared, NEES), as in the localization benchmark.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "execution_count": null,
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      },
      "source": [
        "model.nees_print(left_ukf_nees, right_ukf_nees, ekf_nees)"
      ]
    },
    {
      "source": [
        "All the filters obtain the same NEES and are consistent.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "source": [
        "**Which filter is the best ?** For the considered problem, **left UKF**,\n**right UKF**, and **EKF** obtain the same performances. This is expected as\nwhen the state consists of an orientation only, left and right UKFs are\nimplicitely the same. The EKF obtains similar results as it is also based on a\nretraction build on $SO(3)$ (not with Euler angles). \n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "source": [
        "Conclusion\n==============================================================================\nThis script compares two UKFs and one EKF for the problem of attitude\nestimation. All the filters obtain similar performances as the state involves\nonly the orientation of  the platform.\n\nYou can now:\n\n- compare the filters in different noise setting to see if the filters still\n  get the same performances.\n\n- address the problem of 3D inertial navigation, where the state is defined as\n  the oriention of the vehicle along with its velocity and its position.\n\n"
      ],
      "cell_type": "markdown",
      "metadata": {}
    }
  ],
  "nbformat_minor": 0,
  "metadata": {
    "kernelspec": {
      "name": "python3",
      "language": "python",
      "display_name": "Python 3"
    },
    "language_info": {
      "version": "3.5.2",
      "codemirror_mode": {
        "version": 3,
        "name": "ipython"
      },
      "pygments_lexer": "ipython3",
      "name": "python",
      "file_extension": ".py",
      "nbconvert_exporter": "python",
      "mimetype": "text/x-python"
    }
  },
  "nbformat": 4
}